/*
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     magnet,              sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyroPort,            sensorI2CHiTechnicGyro)

#pragma config(Motor,  mtr_S1_C1_1,     motorFrontMidRight, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorBackRight, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     motorHarvest,  tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorScore,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     motorBackLeft, tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorFrontMidLeft, tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    rghGrab,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    lftGrab,              tServoStandard)
*/
#pragma systemFile

void initmtrs(){
#define motorFrontMidRight mtr_S1_C1_1
  //bMotorReflected[motorFrontMidRight] = true;

#define motorBackRight     mtr_S1_C1_2
  //bMotorReflected[motorBackRight] = true;

#define motorHarvest       mtr_S1_C2_1
  //bMotorReflected[motorHarvest] = true;

#define motorScore         mtr_S1_C2_2
  bMotorReflected[motorScore] = false;

#define motorBackLeft      mtr_S1_C3_1
bMotorReflected[motorBackLeft] = true;

#define motorFrontMidLeft  mtr_S1_C3_2
bMotorReflected[motorFrontMidLeft] = true;

#define rghGrab srvo_S1_C4_1
#define lftGrab srvo_S1_C4_2
#define bpush srvo_S1_C4_3
#define bpushR srvo_S1_C4_5
#define harvester srvo_S1_C4_4
#define magnet S2
#define gyroPort S3
}

void leftmtr(int val){
  motor[motorBackLeft] = val;
  motor[motorFrontMidLeft] = val;
}

void rightmtr(int val){
  motor[motorBackRight] = val;
  motor[motorFrontMidRight] = val;
}

/*
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     magnet,              sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyroPort,            sensorI2CHiTechnicGyro)

#pragma config(Motor,  mtr_S1_C1_1,     motorFrontMidRight, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorBackRight, tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     motorHarvest,  tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorScore,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     motorBackLeft, tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorFrontMidLeft, tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    rghGrab,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    lftGrab,              tServoStandard)
*/
#pragma systemFile
//#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#define joy1Btn(btn) ((joystick.joy1_Buttons & (1 << (btn - 1))) != 0)
#define joy2Btn(btn) ((joystick.joy2_Buttons & (1 << (btn - 1))) != 0)

void forwardForTimeAndStop(int power,int time)
{
  rightmtr(power);
  leftmtr(power);
  wait1Msec(time);
  rightmtr(0);
  leftmtr(0);
}

void turnRightForTimeAndStop(int power,int time)
{
  rightmtr(power);
  leftmtr(-power);
  wait1Msec(time);
  rightmtr(0);
  leftmtr(0);
}

int gyroOff = 600;

#define initGyro() {;;}

void turnRightForGyroAndStop(int power,int gyroDist)
{
  rightmtr(power);
  leftmtr(-power);
  int gyroVal = 0;
  while (abs(gyroVal) < abs(gyroDist)) {
    int deltaGyro = (SensorValue[gyroPort]-gyroOff)/20;
    if (abs(deltaGyro) < 2){
      deltaGyro = 0;
    }
    gyroVal += deltaGyro;
    wait10Msec(2);
  }
  rightmtr(0);
  leftmtr(0);
}

void gyroForwardAndStop(int power,int time)
{
  ClearTimer(T4);
  rightmtr(power);
  leftmtr(power);
  int gyroVal = 0;
  while (time10[T4] < time) {
    int deltaGyro = (SensorValue[gyroPort]-gyroOff)/20;
    if (abs(deltaGyro) < 2){
      deltaGyro = 0;
    }
    gyroVal += deltaGyro;
    rightmtr(power + gyroVal);
    leftmtr(power - gyroVal);
    wait10Msec(2);
  }
  rightmtr(0);
  leftmtr(0);
}
/*
int gyrodSpeed = 0;
int gyrodVal = 0;
task GyroDaemon()
{
  while(joystick.StopPgm);
  while(true){
    int deltaGyro = (SensorValue[gyroPort]-gyroOff)/20;
    /*if (abs(deltaGyro) < 2){
      gyrodval = 0;
      deltaGyro = 0;
    }*//*
    gyrodVal += deltaGyro;
    nxtDisplayCenteredTextLine(2, ""+gyrodVal);
    rightmtr(gyrodSpeed + gyrodVal);
    leftmtr(gyrodSpeed - gyrodVal);
    wait10Msec(2);
  }
}
#define gyrodTurnRight(delta){gyrodVal -= delta;}
#define gyrodMoveForward(speedd){gyrodSpeed = speedd;}

void forwardGyrodForTimeAndStop(int power,int time)
{
  gyrodMoveForward(power);
  wait10Msec(time);
  gyrodMoveForward(0);
}*/

void checkbattery(){
  PlayTone(15000 - externalBattery, 200);
  while(externalBattery < 12000){
    PlayTone(3000, 40);
    PlayTone(8000, 40);
  }
  ClearSounds();
}

// Slow down motors, and create dead zone for controls
/*int adjustval(int in)
{
  if (abs(in) < 16)
    return 0;
  else if (joy1Btn(7))
    return in;
  else
    return in > 0 ? dspeed - cosDegrees(in * 89 / 128) * dspeed : cosDegrees(in * 89 / 128) * dspeed - dspeed;
}*/


task KillSwitch()
{
  nSchedulePriority = 10;
  int i = 1;
  while(true) {
    if ((joy1Btn(1) && joy1Btn(2) && joy1Btn(3) && joy1Btn(4))||(joy2Btn(1) && joy2Btn(2) && joy2Btn(3) && joy2Btn(4))) {
      motor[motorBackLeft] = 0;
      motor[motorFrontMidLeft] = 0;
      motor[motorBackRight] = 0;
      motor[motorFrontMidRight] = 0;
      motor[motorHarvest] = 0;
      motor[motorScore] = 0;
      StopAllTasks();
    }
    wait10Msec(100);
    ++i;
  }
}

const int SCORER_MOVING_EXTENDED = 1;
const int SCORER_MOVING_RETRACTED = 0;
int scorerTransitDir;// false is moving up; true is moving down.
bool scorerMoving;
const int SCORER_EXTENDED_POS = 300;

void extendScorer()
{
  scorerTransitDir = SCORER_MOVING_EXTENDED;
  scorerMoving = true;
}
void retractScorer()
{
  scorerTransitDir = SCORER_MOVING_RETRACTED;
  scorerMoving = true;
}

task ScorerDaemon()
{
  while(joystick.StopPgm);
  while(true)
  {
    if (scorerMoving){
      if (scorerTransitDir == SCORER_MOVING_RETRACTED){//Moving Down
        motor[motorScore] = -15;
        scorerMoving = true;
        if (nMotorEncoder[motorScore] < 10){
          motor[motorScore] = 0;
          scorerMoving = false;
        }else
          PlayImmediateTone(1000,10);
      }else if (scorerTransitDir == SCORER_MOVING_EXTENDED){

        scorerMoving = true;
        motor[motorScore] = 32;
        if (nMotorEncoder[motorScore] > SCORER_EXTENDED_POS){
          motor[motorScore] = 0;
          scorerMoving = false;
        }
      }
    }else{
      motor[motorScore] = 0;
      //PlayImmediateTone(200,10);
    }
    wait10Msec(1);
  }
}

//207 55
#define GUP 207
#define GMID 75
#define GDOWN 59
#define GOFF 262

void gmid(){
      servoTarget[lftGrab] = GOFF-75;
      servoTarget[rghGrab] = 75;
}

void gup(){
  //nxtDisplayCenteredTextLine(2, "not potato");
  servoTarget[lftGrab] = GOFF-GUP;
  servoTarget[rghGrab] = GUP;
}

void gdown(){
  //nxtDisplayCenteredTextLine(2, "super potato");
  /*servoTarget[lftGrab] = GOFF-59;
  servoTarget[rghGrab] = 59;*/
  gmid();
}

void gback(){
  //nxtDisplayCenteredTextLine(2, "ermph potato");
  servoTarget[lftGrab] = GOFF-255;
  servoTarget[rghGrab] = 255;
}

void backRetract(){
  servoTarget[bpush] = 0;
  servoTarget[bpushR] = 207;
}

void backExtend(){
  servoTarget[bpush] = 120;
  servoTarget[bpushR] = 87;
}

void backHyperExtend(){
  servoTarget[bpush] = 207;
  servoTarget[bpushR] = 0;
}
